#include <arpa/inet.h>
#include <unistd.h>
#include <iostream>
#include <cstring>
#include <fstream>
#include <string>
#include <vector>
#include <queue>
#include <algorithm>
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
#include "robot.hpp"
#include "udp.hpp"


using namespace std;
using namespace cv;
void on_MouseHandle(int event, int x, int y, int flags, void* param);//鼠标响应回调函数
float length(Point point1,Point point2);
float get_angle_by_cos(Point p0,Point p1,Point p2);
std::wstring ANSIToUnicode(const std::string & str);
Rect g_select_rect(-1,-1,0,0); //鼠标画的矩形
bool g_drawflag;
bool g_selected_flag;
queue<Point> g_nav_points;
int main()
{
    double fx,fy,cx,cy,k1,k2,k3,p1,p2;
    fx=955.8925;
    fy=955.4439;
    cx=296.9006;
    cy=215.9074;
    k1=-0.1523;
    k2=0.7722;
    k3=0;
    p1=0;
    p2=0;

    Mat cameraMatrix = (cv::Mat_<float>(3, 3) <<
                                              fx, 0.0, cx,
                                              0.0, fy, cy,
                                              0.0, 0.0, 1.0);
    Mat distCoeffs = (cv::Mat_<float>(5, 1) << k1, k2, p1, p2, k3);


    int fd2 = udp_init("192.168.31.42",1234);
    int fd3 = udp_init("192.168.31.226",1234);
    int fd4 = udp_init("192.168.31.75",1234);
    int fd5 = udp_init("192.168.31.138",1234);

    char udp_sendbuff[4][1024];
    char udp_recvbuff[1024];
    //打开视频
    VideoCapture cap(2);
    cap.set(CAP_PROP_FRAME_WIDTH, 640);
    cap.set(CAP_PROP_FRAME_HEIGHT, 480);
    cap.set(CAP_PROP_FPS,30);
    vector<Robot> robots;
    vector<int> all_robots_ids;
    auto dictionary = aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME::DICT_6X6_250);
    auto parameters = aruco::DetectorParameters::create();

    Mat frame;
    if(cap.isOpened())
    {
        cout<<"摄像头打开成功！"<<endl;
    }
    else
    {
        cout<<"摄像头打开失败"<<endl;
        return 0;
    }
    bool already_exist_flag = false;
    while (true)
    {
        vector<vector<Point2f>>corners, rejectedImgPoints;
        vector<int>ids;
        vector<int>selected_ids;
        vector<vector<Point2f>>selected_corners;


        cap.read(frame);
        setMouseCallback("video", on_MouseHandle, (void *) &frame);
        if(g_drawflag)
            rectangle(frame,g_select_rect,Scalar(255,0,0));
        aruco::detectMarkers(frame, dictionary, corners, ids, parameters, rejectedImgPoints);
        
        if(!ids.empty())
        {
            vector<Vec3d> rvecs, tvecs;                     // 储存结果R/t
            aruco::estimatePoseSingleMarkers(corners, 0.055, cameraMatrix, distCoeffs, rvecs, tvecs); // 求解旋转矩阵rvecs和平移矩阵tvecs
            //cout<<"R :"<<rvecs[0]<<endl;
            //cout<<"T :"<<tvecs[0]<<endl;
            int idsize = ids.size();
            for(int i = 0; i<idsize; i++)                                //遍历所有检测到的id
            {
                aruco::drawAxis(frame, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);


                for(vector<Robot>::iterator it = robots.begin();it!=robots.end();it++)      //遍历所有已经有的机器人
                {
                    if(it->get_id() == ids[i])              //判断检测到的ID是否已经存在
                    {
                        it->set_coordinate(cv::Point2f((corners.at(i).at(0).x+corners.at(i).at(2).x)/2,(corners.at(i).at(0).y+corners.at(i).at(2).y)/2));
                        it->set_point0(corners.at(i).at(0));
                        it->set_point3(corners.at(i).at(3));
                        already_exist_flag = true;
                        break;
                    }
                }
                if(!already_exist_flag)             //如果不存在则新建一个机器人
                {
                    Robot nn(ids[i],cv::Point2f((corners.at(i).at(0).x+corners.at(i).at(2).x)/2,(corners.at(i).at(0).y+corners.at(i).at(2).y)/2),corners.at(i).at(0),corners.at(i).at(0));
                    robots.push_back(nn);
                }

                already_exist_flag = false;
            }
        }

        for(auto it = robots.begin();it!=robots.end();it++)
        {
            if(!g_nav_points.empty())
                line(frame,it->get_coordinate(),g_nav_points.front(),Scalar(0,0,255),2);
            cout<<it->get_id()<<":("<<(it->get_coordinate()).x<<","<<(it->get_coordinate()).y<<")"<<endl;
        }
        if(!g_nav_points.empty())
        {
            for(int i = 0;i<g_nav_points.size();i++)
            {
                Point start_point = g_nav_points.front();
                g_nav_points.push(g_nav_points.front());
                g_nav_points.pop();
                Point end_point = g_nav_points.front();
                if(i == g_nav_points.size()-1)
                    break;
                line(frame,start_point,end_point,Scalar(0,0,255),2);
            }
        }
        for(int i=0;i<4;i++)
            sprintf(udp_sendbuff[i],"C0,R18,F-8788");
        float angle=0,distance=0,direction=0;
        if(!g_nav_points.empty())
        {
            Point target = g_nav_points.front();
            for(auto it = robots.begin();it!=robots.end();it++)
            {
                angle = get_angle_by_cos(it->get_point3(),it->get_point0(),target);
                distance = length(it->get_coordinate(),target);
                direction = (target.x-it->get_point3().x)*(it->get_point0().y-it->get_point3().y)-(target.y-it->get_point3().y)*(it->get_point0().x-it->get_point3().x);
                int degree = round(angle*180/3.1415);
                cout<<"("<<it->get_point3().x<<","<<it->get_point3().y<<")"<<endl;
                cout<<"距离:"<<distance<<"   角度："<<degree<<"   方向:"<<direction<<endl;
                if(distance>25)
                    sprintf(udp_sendbuff[it->get_id()-2],"C%d,R%d,F%d",int(distance),degree,int(direction));
                else
                {
                    sprintf(udp_sendbuff[it->get_id()-2],"C%d,R%d,F%d",int(distance),degree,int(direction));
                    g_nav_points.pop();
                }

            }

        }
        if(!selected_ids.empty())
            aruco::drawDetectedMarkers(frame, selected_corners, selected_ids, Scalar(255, 0, 0));
        if(!ids.empty())
            aruco::drawDetectedMarkers(frame, corners, ids, Scalar(0, 255, 0));
        cout<<robots.size()<<endl;

        imshow("video",frame);
        int key = waitKey(10);

        if(key == 27)
            break;
        switch (key) {
            case 'w':
                cout<<"WWWWW"<<endl;
                for(int i=0;i<4;i++)
                    sprintf(udp_sendbuff[i],"C360,R180,F0");
                break;
            case 's':
                cout<<"SSSSS"<<endl;
                for(int i=0;i<4;i++)
                    sprintf(udp_sendbuff[i],"C0,R18,F-8788");
                break;
            case 'a':
                cout<<"AAAAA"<<endl;
                for(int i=0;i<4;i++)
                    sprintf(udp_sendbuff[i],"C360,R40,F5");
                break;
            case 'd':
                cout<<"DDDDD"<<endl;
                for(int i=0;i<4;i++)
                    sprintf(udp_sendbuff[i],"C360,R40,F-5");
                break;
            default:
                break;
        }

        send(fd2,udp_sendbuff[0],strlen(udp_sendbuff[0])+1,0);
        send(fd3,udp_sendbuff[1],strlen(udp_sendbuff[1])+1,0);
        send(fd4,udp_sendbuff[2],strlen(udp_sendbuff[2])+1,0);
        send(fd5,udp_sendbuff[3],strlen(udp_sendbuff[3])+1,0);

    }
    
    close(fd2); //关闭套接字
    close(fd3); //关闭套接字
    close(fd4); //关闭套接字
    close(fd5); //关闭套接字
    return 0;
}

float get_angle_by_cos(Point p0,Point p1,Point p2)
{
    float angle=0;
    int l11=p0.x-p1.x;
    int l12=p0.y-p1.y;
    int l21=p2.x-p1.x;
    int l22=p2.y-p1.y;
    float m = sqrt(l11*l11+l12*l12)*sqrt(l21*l21+l22*l22);
    if(m==0) return 0;
    float cos = (l11*l21+l12*l22)/m;
    if(cos>=-1 && cos<=1)
        angle = acos(cos);
    else angle =180;
    return angle;
}
float length(Point point1,Point point2)
{
    return sqrt((point1.x-point2.x)*(point1.x-point2.x)+ (point1.y-point2.y)*(point1.y-point2.y));
}
void on_MouseHandle(int event, int x, int y, int flags, void* param)
{
    Mat frame = *(Mat*)param;

    switch (event)
    {
        case EVENT_LBUTTONDOWN:
            g_nav_points.push(Point(x,y));
            break;
        case EVENT_LBUTTONUP:

            break;
        case EVENT_MOUSEMOVE:

            break;
        case EVENT_RBUTTONDOWN:
            g_nav_points = queue<Point>();
            break;
        case EVENT_RBUTTONUP:
            /* code */
            break;
        default:
            break;
    }

}

std::wstring ANSIToUnicode(const std::string & str)
{
    std::wstring ret;
    std::mbstate_t state = {};
    const char *src = str.data();
    size_t len = std::mbsrtowcs(nullptr, &src, 0, &state);
    if (static_cast<size_t>(-1) != len) {
        std::unique_ptr< wchar_t [] > buff(new wchar_t[len + 1]);
        len = std::mbsrtowcs(buff.get(), &src, len, &state);
        if (static_cast<size_t>(-1) != len) {
            ret.assign(buff.get(), len);
        }
    }
    return ret;
}

